
#include "sysClkConfig.h"



static ErrorStatus HSEStartUpStatus;
static ErrorStatus HSIStartUpStatus;


enum
{
    SYSCLK_PLLSRC_HSI,
    SYSCLK_PLLSRC_HSIDIV2,
    SYSCLK_PLLSRC_HSI_PLLDIV2,
    SYSCLK_PLLSRC_HSIDIV2_PLLDIV2,
    SYSCLK_PLLSRC_HSE,
    SYSCLK_PLLSRC_HSEDIV2,
    SYSCLK_PLLSRC_HSE_PLLDIV2,
    SYSCLK_PLLSRC_HSEDIV2_PLLDIV2,
};


void SetSysClockToPLL(u32 freq, u8 src)
{
    u32 pllsrcclk;
    u32 pllsrc;
    u32 pllmul;
    u32 plldiv = RCC_PLLDIVCLK_DISABLE;
    u32 latency;
    u32 pclk1div, pclk2div;
    u32 msi_ready_flag = RESET;

    if (HSE_VALUE != 8000000)
    {
        /* HSE_VALUE == 8000000 is needed in this project! */
        while (1);
    }

    /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration
     * -----------------------------*/

    if ((src == SYSCLK_PLLSRC_HSI)         || (src == SYSCLK_PLLSRC_HSIDIV2) 
     || (src == SYSCLK_PLLSRC_HSI_PLLDIV2) || (src == SYSCLK_PLLSRC_HSIDIV2_PLLDIV2))
    {
        /* Enable HSI */
        RCC_ConfigHsi(RCC_HSI_ENABLE);

        HSIStartUpStatus = RCC_WaitHsiStable();

        if (HSIStartUpStatus != SUCCESS)
        {
            while (1);
        }

        if ((src == SYSCLK_PLLSRC_HSIDIV2) || (src == SYSCLK_PLLSRC_HSIDIV2_PLLDIV2))
        {
            pllsrc = RCC_PLL_HSI_PRE_DIV2;	
            pllsrcclk = HSI_VALUE/2;		

            if(src == SYSCLK_PLLSRC_HSIDIV2_PLLDIV2)
            {
                plldiv = RCC_PLLDIVCLK_ENABLE;	
                pllsrcclk = HSI_VALUE/4;		
            }
        } else if ((src == SYSCLK_PLLSRC_HSI) || (src == SYSCLK_PLLSRC_HSI_PLLDIV2)){
            pllsrc = RCC_PLL_HSI_PRE_DIV1;	
            pllsrcclk = HSI_VALUE;			
            if(src == SYSCLK_PLLSRC_HSI_PLLDIV2)
            {
                plldiv = RCC_PLLDIVCLK_ENABLE;	
                pllsrcclk = HSI_VALUE/2;		
            }
        }

    }else if ((src == SYSCLK_PLLSRC_HSE)         || (src == SYSCLK_PLLSRC_HSEDIV2) 
            || (src == SYSCLK_PLLSRC_HSE_PLLDIV2) || (src == SYSCLK_PLLSRC_HSEDIV2_PLLDIV2))
    {
        /* Enable HSE */
        RCC_ConfigHse(RCC_HSE_ENABLE);


        HSEStartUpStatus = RCC_WaitHseStable();

        if (HSEStartUpStatus != SUCCESS)
        {
            /* If HSE fails to start-up, the application will have wrong clock
               configuration. User can add here some code to deal with this
               error */

            /* Go to infinite loop */
            while (1);
        }

        if ((src == SYSCLK_PLLSRC_HSEDIV2) || (src == SYSCLK_PLLSRC_HSEDIV2_PLLDIV2))
        {
            pllsrc = RCC_PLL_SRC_HSE_DIV2;
            pllsrcclk = HSE_VALUE/2;

            if(src == SYSCLK_PLLSRC_HSEDIV2_PLLDIV2)
            {
                plldiv = RCC_PLLDIVCLK_ENABLE;
                pllsrcclk = HSE_VALUE/4;
            }
        } else if ((src == SYSCLK_PLLSRC_HSE) || (src == SYSCLK_PLLSRC_HSE_PLLDIV2))
        {
            pllsrc = RCC_PLL_SRC_HSE_DIV1;
            pllsrcclk = HSE_VALUE;

            if(src == SYSCLK_PLLSRC_HSE_PLLDIV2)
            {
                plldiv = RCC_PLLDIVCLK_ENABLE;
                pllsrcclk = HSE_VALUE/2;
            }
        }
    }

    latency = (freq/32000000);
    
    if(freq > 54000000)
    {
        pclk1div = RCC_HCLK_DIV4;
        pclk2div = RCC_HCLK_DIV2;
    }else{
        if(freq > 27000000)
        {
            pclk1div = RCC_HCLK_DIV2;
            pclk2div = RCC_HCLK_DIV1;
        }else {
            pclk1div = RCC_HCLK_DIV1;
            pclk2div = RCC_HCLK_DIV1;
        }
    }
    
    if(((freq % pllsrcclk) == 0) && ((freq / pllsrcclk) >= 2) && ((freq / pllsrcclk) <= 32))
    {
        pllmul = (freq / pllsrcclk);
        if(pllmul <= 16)
        {
            pllmul = ((pllmul - 2) << 18);
        }else{
            pllmul = (((pllmul - 17) << 18) | (1 << 27));
        }
    }else{
        dbgsegprint("Cannot make a PLL multiply factor to freq..\n");
        while(1);
    }

    if(RESET == RCC_GetFlagStatus(RCC_CTRLSTS_FLAG_MSIRD))
    {
        RCC_ConfigMsi(RCC_MSI_ENABLE, RCC_MSI_RANGE_4M);
        while(SUCCESS != RCC_WaitMsiStable());

        msi_ready_flag = SET;
    }

    RCC_ConfigSysclk(RCC_SYSCLK_SRC_MSI);

    FLASH_SetLatency(latency);

    /* HCLK = SYSCLK */
    RCC_ConfigHclk(RCC_SYSCLK_DIV1);

    /* PCLK2 = HCLK */
    RCC_ConfigPclk2(pclk2div);

    /* PCLK1 = HCLK */
    RCC_ConfigPclk1(pclk1div);

    /* Disable PLL */
    RCC_EnablePll(DISABLE);

    RCC_ConfigPll(pllsrc, pllmul, plldiv);

    /* Enable PLL */
    RCC_EnablePll(ENABLE);

    /* Wait till PLL is ready */
    while (RCC_GetFlagStatus(RCC_CTRL_FLAG_PLLRDF) == RESET);

    /* Select PLL as system clock source */
    RCC_ConfigSysclk(RCC_SYSCLK_SRC_PLLCLK);

    /* Wait till PLL is used as system clock source */
    while (RCC_GetSysclkSrc() != 0x0C);

    if(msi_ready_flag == SET)
    {
        /* MSI oscillator OFF */
        RCC_ConfigMsi(RCC_MSI_DISABLE, RCC_MSI_RANGE_4M);
    }
}


void sysClkConfig(void)
{
		//GPIO_InitType GPIO_InitStruct_1;
		//NVIC_SetVectorTable(FLASH_START_ADDR,FLASH_OFFSET);
		SetSysClockToPLL(108000000, SYSCLK_PLLSRC_HSIDIV2_PLLDIV2);		
#if 0
		RCC_ConfigMcoClkPre(RCC_MCO_CLK_NUM15);
		RCC_ConfigMco(RCC_MCO_SYSCLK);
		RCC_EnableAPB2PeriphClk(RCC_APB2_PERIPH_GPIOA, ENABLE);
		GPIO_InitStruct(&GPIO_InitStruct_1);
		GPIO_InitStruct_1.Pin			  = GPIO_PIN_8;
		GPIO_InitStruct_1.GPIO_Mode 	  = GPIO_Mode_AF_PP;
		GPIO_InitStruct_1.GPIO_Alternate  = GPIO_AF8_MCO;
		GPIO_InitPeripheral(GPIOA, &GPIO_InitStruct_1);
#endif	
		dbgsegprint("\r\n[DBG_UPGRADE_TX] UPGRADE_DOWNLOAD_EVT\tint[%d]\r\n",sizeof(int));
		//SEGGER_RTT_printf(0,"\r\n[DBG_UPGRADE_TX] UPGRADE_DOWNLOAD_EVT\tint[%d]\r\n",sizeof(int));
}






void print_system_clocks(void)
{
    RCC_ClocksType RCC_Clocks;
    
    RCC_GetClocksFreqValue(&RCC_Clocks);
    
    dbgsegprint( "System Clock Information:\r\n");
    dbgsegprint( "-------------------------\r\n");
    dbgsegprint( "SYSCLK: %d Hz\r\n", RCC_Clocks.SysclkFreq);
    dbgsegprint( "HCLK  : %d Hz\r\n", RCC_Clocks.HclkFreq);
    dbgsegprint( "PCLK1 : %d Hz\r\n", RCC_Clocks.Pclk1Freq);
    dbgsegprint( "PCLK2 : %d Hz\r\n", RCC_Clocks.Pclk2Freq);
    dbgsegprint( "ADCPLL: %d Hz\r\n", RCC_Clocks.AdcPllClkFreq);
    dbgsegprint( "-------------------------\r\n");
}
